{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "from robot import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Robotics, vision and control fundamental algorithms in MATLAB"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. Representing Position and Orientation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P22\n",
    "# 二维中移动 (1,2) 并转动 30 度的齐次变换矩阵(homogeneous)\n",
    "se=se2(1,2,30*pi/180)\n",
    "Matrix(se)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P27, 绕 x 轴转动\n",
    "# x - 红\n",
    "# y - 绿\n",
    "# z - 蓝\n",
    "R = rotx(pi/2)\n",
    "trplot(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P28, 先绕y转再绕x转\n",
    "R = rotx(pi/2) * roty(pi/2)\n",
    "trplot(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# 先绕x转再绕y转\n",
    "R = roty(pi/2) * rotx(pi/2)\n",
    "trplot(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P29, eul 表示\n",
    "R=eul2tr(0.1,0.2,0.3)\n",
    "Matrix(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R=trotz(0.1)*troty(0.2)*trotz(0.3)\n",
    "Matrix(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "eul = tr2eul(eul2tr(0.1,0.2,0.3))\n",
    "Matrix(eul)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P29, 对任意转动矩阵的 tr2eul 结果并不唯一\n",
    "eul = tr2eul(eul2tr(0.1,-0.2,0.3))\n",
    "Matrix(eul)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  rpy\n",
    "\n",
    "$$\n",
    "R = R_x(\\theta_r)R_y(\\theta_p)R_z(\\theta_y)\n",
    "$$\n",
    "\n",
    "$\\theta_p = \\pm \\frac{\\pi}{2}$ 时奇异"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# P30, rpy 表示,  时奇异\n",
    "T = rpy2tr(0.1,0.2,0.3)\n",
    "Matrix(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T = trotz(0.1)*troty(0.2)*trotx(0.3) # 同 rpy2tr\n",
    "Matrix(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "assert (rpy2tr(0.1,0.2,0.3) == trotz(0.1)*troty(0.2)*trotx(0.3)).all()\n",
    "assert not (rpy2tr(0.1,0.2,0.3) == trotz(0.3)*troty(0.2)*trotx(0.1)).all()\n",
    "assert not (rpy2tr(0.1,0.2,0.3) == trotx(0.1)*troty(0.2)*trotz(0.3)).all()\n",
    "assert not (rpy2tr(0.1,0.2,0.3) == trotx(0.3)*troty(0.2)*trotz(0.1)).all()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rpy = tr2rpy(rpy2tr(0.1,0.2,0.3))\n",
    "Matrix(rpy)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 绕定轴转动\n",
    "\n",
    "$$\n",
    "R=I_{ 3 \\times 3 } + \\sin \\theta S(v) + (1 - \\cos \\theta)( v v^T - I_{ 3 \\times 3 })\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R = rotvec2tr(pi/2, [1,0,0])\n",
    "Matrix(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "tr2rotvec(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "R.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  四元数\n",
    "\n",
    "$$\n",
    "q = s + v\n",
    "$$\n",
    "\n",
    "$$\n",
    "v = v_1 i + v_2 j + v_3 k\n",
    "$$\n",
    "\n",
    "$$\n",
    "i^2=j^2=k^2=ijk=-1\n",
    "$$\n",
    "\n",
    "$$\n",
    "q=s<v_1, v_2, v_3>\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "quaternion(rpy2tr(0.1,0.2,0.3)) # P35, 与书上结果不同"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "quaternion(rotx(0.1)*roty(0.2)*rotz(0.3)) # P35, 与书上结果相同"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "结论:\n",
    "在机器人库中,\n",
    "\n",
    "``` python\n",
    "rpy2tr(y,p,r) = trotz(y)*troty(p)*trotx(r)\n",
    "```\n",
    "\n",
    "在 pdf 图书中,\n",
    "\n",
    "``` python\n",
    "rpy2tr(r,p,y) = trotx(r)*troty(p)*trotz(y)\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q = quaternion(rotx(0.1)*roty(0.2)*rotz(0.3))\n",
    "q.norm()  # 模"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q.inv() # 拟"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q*q.inv() # 应该为 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q/q # 应该为 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q.r() # 旋转矩阵"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "q.tr() # 齐次矩阵"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "v = mat([1,0,0]) # 列向量\n",
    "q*v # 对列向量作用 q 变换"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T = transl(1, 0, -1) * trotx(pi/2) * transl(0, 1, 0)\n",
    "transl(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "T = transl(1, 0, 0) * trotx(pi/2) * transl(0, 1, 0)\n",
    "trplot(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
